#include <stdlib.h>
#include <iostream>
using std::cout;
using std::endl;

#include "path.h"

#ifndef _PATHPLANNER_H_
#define _PATHPLANNER_H_

class PathPlanner {
public:
	// Constructor
	PathPlanner(int **map, int x, int y, Point &start, Point &end) 
		: _map(map), _x(x), _y(y), _start(start), _end(end) { 
	}

	// Destructor
	~PathPlanner() {
		for(int i=0; i<_y; i++) {
			delete[] _map[i];
		}
		delete[] _map;
		delete _path;
	}

	// Perform A* on map
	void astar() {
		Point curr = _start;

		// The set of nodes already evaluated
		Path *closedSet = NULL;

		// The set of tentative nodes to be evaluated
		Path *openSet = NULL;
		openSet = addToEndOfPath(openSet, curr);
	
		do {
			// Find point with lowest F-Score
                        curr = findLowestFScore(openSet);

			// Add adjacent points to open set
			openSet = addAdjacentSquaresToQueue(openSet, closedSet, curr);
			if(openSet == NULL) {
				_path = NULL;
				return;
			}

			// Add current point to closed set and remove from open set
			openSet = removeFromPath(openSet, curr);
			closedSet = addToEndOfPath(closedSet, curr);

			// Calculate F score
			calculateFScores(openSet);
		} while(!inPath(closedSet, _end));

		_end = findEndPoint(closedSet);

		_path = reconstructPath(closedSet);

		delete closedSet;
		delete openSet;
	}

	// Get the path
	Path* getPath() {
		return _path;
	}

	// Print the map and path
	void print() {
		cout << "\033[31m";
		cout << "Start point: (" << _start.x << ", " << _start.y << ")\t"; 
		cout << "End point: (" << _end.x << ", " << _end.y << ")" << endl << endl; 

		for(int i=0; i<_y; i++) {
			for(int j=0; j<_x; j++) {
				if(_map[i][j]) {
					cout << _map[i][j] << " ";
				} else if(inPath(_path, Point(j, i))) {
					cout << "P ";
				} else {
					cout << "- ";
				}
			}
			cout << endl;
		}

		cout << endl << "Shortest path: ";
		printPath(_path, false);
		cout << "\033[0m";
	}

private:
	Path* addAdjacentSquaresToQueue(Path *open, Path *closed, Point p) {
		if(open == NULL) {
			return open;
		}

		int starti = p.y > 0 ? p.y-1 : p.y;
		int endi = p.y < (_y-1) ? p.y+1 : p.y;
		int startj = p.x > 0 ? p.x-1 : p.x;
		int endj = p.x < (_x-1) ? p.x+1 : p.x;

		for(int i=starti; i<=endi; i++) {
			for(int j=startj; j<=endj; j++) {
				// Ignore current point and obstacles
				if((i != p.y || j != p.x) && _map[i][j] != 1) {
					// Don't add if it's already in open or closed set
					if(!inPath(open, Point(j, i)) && !inPath(closed, Point(j, i)) ) {
						open= addToEndOfPath(open, Point(j, i, p.x, p.y, p.gScore));
					}

					// If on open list, see if this path is better
					else if(inPath(open, Point(j, i))) {
						Point tmp = findPoint(open, j, i);
						recalculateScores(tmp, p, open);
					} 
				}
			}
		}
		return open;
	}

	void calculateFScores(Path *path) {
		if(path != NULL) {
			Point p = path->p;

			// Calculate G-Score
			if(p.x != p.px && p.y != p.py) {
				path->p.gScore = 14 + p.pgScore;
			} else {
				path->p.gScore = 10 + p.pgScore;
			}
			
			// Diagonal Method - Admissable, but slower
			int h;
			int xDist = abs(p.x - _end.x);
			int yDist = abs(p.y - _end.y);
			if(xDist > yDist) {
				h = 14 * yDist + 10 * (xDist - yDist);
			} else {
				h = 14 * xDist + 10 * (yDist - xDist);
			}
		
			// Calculate F-Score
			path->p.fScore = path->p.gScore + h;

			calculateFScores(path->next); 
		}
	}

	Path* recalculateScores(Point p, Point parent, Path *path) {
		int gScore;

		// Calculate G-Score
		if(p.x != parent.x && p.y != parent.y) {
			gScore = 14 + parent.gScore;
		} else {
			gScore = 10 + parent.gScore;
		}

		// Check if this gScore is better
		if(gScore < p.gScore) {
			path = removeFromPath(path, p);
			p.px = parent.x;
			p.py = parent.y;
			p.fScore = p.fScore - p.gScore + gScore;
			p.gScore = gScore;
			p.pgScore = parent.gScore;
			path = addToEndOfPath(path, p);
		}

		return path;
	}


	Point findLowestFScore(Path *path) {
		if(path != NULL) {
			Point p = path->p;
			Point p_low = findLowestFScore(path->next);

			return (p.fScore < p_low.fScore ? p : p_low);
		} else { 
			Point p(-1, -1);
			p.fScore = 999999;
			return p;
		}
	}

	Point findEndPoint(Path* path) {
		while(path->next != NULL) {
			path = path->next;
		}
		return path->p;
	}

	Path* reconstructPath(Path *path) {
		Path *ret = NULL;
		Point curr = _end;

		do {
			ret = addToStartOfPath(ret, curr);
			curr = findParent(path, curr);
		} while(!(curr.x == _start.x && curr.y == _start.y));

		return addToStartOfPath(ret, _start);
	}

	int abs(int val) {
		return (val < 0 ? 0-val : val);
	}

	int **_map;
	int _x;
	int _y;
	Path *_path;
	Point _start;
	Point _end;
};

#endif
